#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include "geometry_msgs/PoseStamped.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/String.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <actionlib/client/simple_action_client.h>
using namespace std;


#define PREVIEW_DIS 0.8  //预瞄距离
#define Ld 0.35  //前后轮中心---轴距

ros::Publisher purepersuit_pub_, path_pub_, initPose_pub_;

nav_msgs::Path path;
geometry_msgs::Pose start_pose_;
geometry_msgs::Point current_point_;

double end_point_x_,end_point_y_;
double k = 0.1;
double carVelocity = 0;
double preview_dis = 0;

bool recv_path_flag = true;
bool path_end = false;

std::vector<float> r_x_;
std::vector<float> r_y_;
std::array<float,3> calRPY_;

bool loop = true; //循环运动标志，默认开启

double currentPositionX, currentPositionY, currentPositionZ;
double currentQuaternionX, currentQuaternionY, currentQuaternionZ, currentQuaternionW;

#define robot_startWait  0   //
#define robot_followPth  1   // 
#define robot_loop 2 //
int robot_state  = robot_startWait;

/**
 * 四元数转欧拉角
*/
std::array<float, 3> calQuaternionToEuler(const float x, const float y,
                                          const float z, const float w) 
{
  std::array<float, 3> calRPY = {(0, 0, 0)};
  // roll = atan2(2(wx+yz),1-2(x*x+y*y))
  calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
  // pitch = arcsin(2(wy-zx))
  calRPY[1] = asin(2 * (w * y - z * x));
  // yaw = atan2(2(wx+yz),1-2(y*y+z*z))
  calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));

  return calRPY;
}

void pathPointCallback(const nav_msgs::Path::ConstPtr &msg)
{
    int g_pointNum = msg->poses.size();   
    if( (g_pointNum >1) && (recv_path_flag==true))
    {
        recv_path_flag  = false;

        end_point_x_ = msg->poses[g_pointNum-1].pose.position.x;
        end_point_y_ = msg->poses[g_pointNum-1].pose.position.y;

        start_pose_= msg->poses[0].pose;

        for (int i = 0; i < g_pointNum; i++)
        {
            r_x_.push_back(msg->poses[i].pose.position.x);
            r_y_.push_back(msg->poses[i].pose.position.y);

        }
        robot_state = robot_followPth;

    }
}
/*
 * 速度回调函数，根据当前速度计算实际前视距离
*/
void velocityCallBack(const geometry_msgs::Twist::ConstPtr& carVel)
{
    carVelocity = carVel->linear.x ;
    preview_dis = k*carVelocity + PREVIEW_DIS;
}
void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& amcl_pose)
{
    currentPositionX = amcl_pose->pose.pose.position.x;
    currentPositionY = amcl_pose->pose.pose.position.y;
    currentPositionZ = 0.0;
    ROS_INFO("curr_location: %f,%f",currentPositionX,currentPositionY);
    currentQuaternionX = amcl_pose->pose.pose.orientation.x;
    currentQuaternionY = amcl_pose->pose.pose.orientation.y;
    currentQuaternionZ = amcl_pose->pose.pose.orientation.z;
    currentQuaternionW = amcl_pose->pose.pose.orientation.w;
    calRPY_ = calQuaternionToEuler(currentQuaternionX,currentQuaternionY,
                                   currentQuaternionZ,currentQuaternionW );
}

void cmdVelPub(double linear_x,double angular_z)
{
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = linear_x;
    vel_msg.angular.z = angular_z;
    purepersuit_pub_.publish(vel_msg);
    ros::Duration(0.01).sleep();
}

int main(int argc,char** argv)
{
    ros::init(argc,argv,"pure_pursuit");
    ros::NodeHandle n;
    purepersuit_pub_ = n.advertise<geometry_msgs::Twist>("/ackermann_steering_controller/cmd_vel",10);
    initPose_pub_ = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose",10); 
    path_pub_ = n.advertise<nav_msgs::Path>("rvizpath", 100, true);

    ros::Subscriber path_sub_ = n.subscribe("/path_pub", 1000, pathPointCallback);   //路径接收函数
    ros::Subscriber carVel = n.subscribe("/ackermann_steering_controller/cmd_vel", 10, velocityCallBack);
    ros::Subscriber carPose = n.subscribe("/amcl_pose", 10, robotPoseCallBack);

    path.header.frame_id = "/map";
    // 设置时间戳
    path.header.stamp = ros::Time::now();
    ros::Rate r(50);
    while (ros::ok())
    {
        switch (robot_state)
        {
            case robot_startWait:
            {
                ROS_INFO("robot start wait outgoing");
                break;
            }
            case robot_followPth:
            {
                //判断机器自身和路径终点距离
                if(abs(currentPositionX - end_point_x_) <0.2 && abs(currentPositionY - end_point_y_)< 0.2 )
                {
                    ROS_INFO("arrive at the end point");
                    if(loop == false)
                    {
                        path_end = true;
                        cmdVelPub(0,0);
                    }
                    // else
                }  
                if(path_end == false)
                {
                    std::vector<double> bestPoints_;
                        for(int i = 0; i<r_x_.size(); i++)
                        {
                            float path_x = r_x_[i];
                            float path_y = r_y_[i];
                            // 遍历所有路径点和当前位置的距离，保存到数组中
                            float lad = sqrt(pow(path_x - currentPositionX, 2) +
                                            pow(path_y - currentPositionY, 2));
                            bestPoints_.push_back(lad);
                        }    
                        //找到数组中最小距离
                        auto smallest = min_element(bestPoints_.begin(), bestPoints_.end());
                        //找到最小距离的索引
                        int index = distance(bestPoints_.begin(),smallest);
                        int temp_index = 0;    
                        //从最小距离的位置开始，遍历路径点 到 实际前视距离的距离
                        for(int i = index ; i<r_x_.size(); i++)
                        {
                            float dis = sqrt(pow(r_y_[index] - r_y_[i], 2) + pow(r_x_[index] - r_x_[i], 2));
                            if(dis < preview_dis)
                            {
                                temp_index = i;
                            }
                            else
                                break;
                        }
                        index = temp_index;
                        float alpha =
                            atan2(r_y_[index] - currentPositionY, r_x_[index] - currentPositionX) -
                            calRPY_[2];
                        // 当前点和目标点的距离dl
                        float dl = sqrt(pow(r_y_[index] - currentPositionY, 2) +
                                        pow(r_x_[index] - currentPositionX, 2));
                        // 发布小车运动指令及运动轨迹
                        if( dl>0.05 )
                        {
                            float theta = atan(2 * Ld * sin(alpha) / dl);
                            cmdVelPub(0.5,theta); 
                        // 发布小车运动轨迹
                            geometry_msgs::PoseStamped this_pose_stamped;
                            this_pose_stamped.pose.position.x = currentPositionX;
                            this_pose_stamped.pose.position.y = currentPositionY;

                            geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(theta);
                            this_pose_stamped.pose.orientation.x = currentQuaternionX;
                            this_pose_stamped.pose.orientation.y = currentQuaternionY;
                            this_pose_stamped.pose.orientation.z = currentQuaternionZ;
                            this_pose_stamped.pose.orientation.w = currentQuaternionW;

                            this_pose_stamped.header.stamp = ros::Time::now();

                            this_pose_stamped.header.frame_id = "map";
                            path.poses.push_back(this_pose_stamped);                    
                        }
                        else
                        {
                            cmdVelPub(0,0);
                        }   
                        path_pub_.publish(path);        
                }
                break;
            }      
            default:
                break;   
        }
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}
